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ABSTRACT 


This thesis applies extended Kalman filtering to the problem of estimating frequency, ampli- 
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I. INTRODUCTION 


The detection, estimation and tracking of signals plays a significant role in many 
aspects of military and civilian operations. The Kalman filter has been used in tracking 
problems for many years. Its power comes from the mathematical foundation of statistical 
optimality. We investigate the behavior of the extended Kalman filter instead of using a 
linear Kalman filter, as most of the real world problems are non-linear. This thesis studies 
the detection of non-periodic signals, corrupted by zero mean, white, Gaussian noise using 
an extended Kalman filter algorithm. The parameters of the signals such as fundamental 
frequency, harmonic amplitudes and phases, are considered unknown and are estimated by 
the algorithm. 

In nature, most physical signals are approximately harmonic. This problem therefore, 
has applications in many fields including those of Radar and Sonar. An example of this is 
in the tracking of propeller driven platforms for submarines and helicopters. The sound of 
a rotary engine is almost periodic. If the engine speed changes, the sound of rotation 
changes, resulting in a change of frequency. The amplitudes and phases may also change 
slowly over time. Knowing the frequency, amplitude and the phase of the signal may allow 
identification of the class of vessel generating the sound. The frequency of the detected 
sound tells the speed of the vessel. The amplitude identifies the amount of vibration. 

Other applications include the estimation of harmonic signal parameters in the 
presence of noise to determine a radar's modulated pulse repetition frequency or to 


investigate noisy biological signals such as heart wave forms. 


This thesis is organized into six chapters. Chapters I and III explain the development 
of the Kalman filter and show the mathematical derivation of the extended Kalman filter 
algorithm. Chapters IV and V model the physical signal and show the simulation of each 
model. Chapter VI gives the conclusion of the report. The appendices contain program code 


applicable to this work. 


Il. KALMAN FILTER 


The object of this chapter is to show the development of the Kalman filter for 
estimating states from noisy data. The Kalman filter has the desirable quality of 
maintaining the physical meaning of the system dynamics by utilizing a state space 
representation. Here the state space model is used as the basic model. 

The optimality of the filter holds for systems which are linear and time-invariant 
(LTD), and corrupted by additive white Gaussian noise (AWGN). This chapter develops the 
Kalman filter equations for such a system. The development follows closely those given by 


Lewis, Gelb, and Candy [Ref. 2, 3, 4]. 


A. DEFINITION OF TERMS 

All of the terms used throughout these chapters are defined in Table 2.1 and will be 
explained as they occur in the derivation. 

Terms with a single time subscript (i.e., x,) refer to the value of the term at that time. 
Terms with dual subscripts (i.e., x, , , ,) refer to the value of the term at the time of the first 
subscript given observations through the second subscript. The third column of Table 2.1 


gives the dimension for each entry. 


TABLE 2.1: DEFINITION OF TERMS 


System order: j 

Observation size: m 

System state: f jo 
Transpose of state sé 1xj 
State transition matrix: © jxj 
State excitation noise: W, jul 
Observation: 2 mxl 
Observation noise: U, mx1 
State estimate: Eli jxa 
Estimate error: XE jx1 
Expected value of the error: E [x,, y 4] DEC I 
Error covariance matrix: DS BE 
Residual: n mx1 


B. SYSTEM DYNAMICS 


A good model of the system 1s necessary for estimating parameters of that system. The 
state space representation of our linear system is given in Equation (2.1), and the measure- 


ment process is modeled as in Equation (2.2). 


Xp, > bayt, (221) 


2, = Hep vy (2.2) 


The physical state of the system (amplitude, frequency, etc.) 1s described by x and the 
Observed parameters (amplitude, frequency, etc.) are described by observation z. Since the 
described system is time invariant, both Y and A are independent of time. 

The noise processes are considered as stationary, uncorrelated, zero-mean, additive 
white Gaussian noise (AWGN) processes. The statistical properties of the noise processes 


are given below 


E[w,] =0 (2.3) 

E [w w; | = A8, ( 2.4) 
W,- (0,8) (25) 
Elv,l 2 0 ( 2.6) 
Bu) = Rô, (2 7) 
v, ~ (0, R) ( 2.8) 

E [w,v]] - 0 (29) 


where © is the kronecker delta function, defined by: 


A cen” (2.10) 


jk VST 


The matrices Q and R are non-zero, diagonal noise covariance matrices, which denote 


the power of the noise of the system. 


C. RECURSIVE SYSTEM 


Before presenting the detail of the Kalman filter equations, we will first introduce the 


linear and recursive forms of the filter, as shown in Equations (2.11) and (2.12). 


X, l| - Ky. (> 0 


Derler A, yari (212) 


K, is a system matrix, K> and G are the time-varying weighting matrices. The current 


estimate is a combination of the previous estimate and the current observation. 


D. DISCRETE KALMAN FILTER 

The Kalman filter may be derived by optimizing the assumed form of the linear esti- 
mator. An optimal system is generally considered to be any system which either minimizes 
a cost function or optimizes a performance function. 

This experiment required the establishment of a filter that gives an unbiased estimate 
and minimizes error. An unbiased estimate has an expected error value of zero. The Con- 
stants K} in Equation (2.11) and K» in Equation (2.12) are chosen to make the estimate un- 
biased. The constant G is chosen to minimize a cost function of the expected error. 

Following measurement, an equation for the estimation error can be obtained from 
Equation (2.12) by substitution of the measurement Equation (2.2) and the defining the re- 


lations as (tilde denotes estimation error): 


Xkalk C Zk+ 1 ŽR+ 1 kl (2.13) 


Xpelke1 7 “poi Ák+alk+l: (2.14) 


The expected values satisfy 


E [xy mecs] x E [Xp4y k] =0 (213) 


and 


Mace ATO — [Ko + GH - I] x , ai 4t Rox, ua t Kov, ( 2.16) 


By taking the expected value of both sides and letting E [x,,,,,] - 0, E[v,] - 0, and 
E|[x,,,1 441] 7 0 (i.e. unbiased estimator), the term in square brackets in Equation (2.16) 


is required to be zero. 


n EH ( 2.17) 


Combining Equation (2.14) and (2.17) gives the estimator the form of 


Zoe O see (2.18) 


or, alternatively 


X, eco eap Qe coo EX. cape (27) 


Following the same procedure, the value of K, can be determined by substituting 


Equations (2.1) and (2.11) into Equation (2.13) as below: 


Xk l| k — DA Re b (2.20) 


By taking the expected value of both sides and applying Equations (2.3) and (2.15), 


we get 


so that Equation (2.11) can be written in the form 


en = DXy z- (229) 


Eguation (2.22) is known as the time update portion of the algorithm and gives the 


prediction x,,,,, of the state at time k+1, along with the associated error covariance 
P, , Also, Equation (2.19) is known as measurement update portion which provides a 
correction based on the measurement z,, , at time k+1 to yield the net a posteriori estimate 
of x,,, and its error covariance of P,,,,,,. This is known as predictor-corrector formu- 


lation of the discrete Kalman filter. 


E. ERROR COVARIANCE UPDATE 


To obtain a measure of confidence for the estimate, we need to find the error covari- 


ance. The error covariance matrices are defined by Equations (2.23) and (2.24). 


Zm kT E [em m k] ( 2.23) 


TARTE El e ri (2.24) 


The corresponding estimation error is of the form 


Xp ij hel = [I - GH] x, y 4 + Gu, (225) 


Rewriting Eguation (2.24) as 


m E - GH)x,. y [X ij (1 GH)7+ 0, G7) + Gv, lx, 4 (0 7 GH)" «1G! ]] 
( 2.26) 


and using Equations (2.23), (2.7), and the result of uncorrelated measurement errors we 


have, 


> 27) 


E | X, , i| 4U4] = E [4X4 y 4] zu 


and 
EW UEG) BA (SGH) GRG: (2.28) 


The error covariance matrix gives the expected magnitude of the estimation error. 


F. OPTIMUM CHOICE OF KALMAN GAIN 
The criterion for choosing the gain is to minimize a weighted scalar sum of the diag- 


onal elements of the error covariance matrix. We will choose the value of G which satisfies 


Equation (2.29). 


(229) 


a > >T 
G:min!c (J,,,] = Eeee) 


Eguation (2.29) can be written in terms of the error covarlance as 


(2.30) 


Gina id). — trace (Pow 


This is equivalent to minimizing the length of the estimation error vector. To find the 


value of the gain which provides a minimum, it is necessary to take the partial derivative 


of J,,, with respect to G and set it to zero. Taking the partial derivative of J,,, requires 


the use of the matrices A and B, where B is symmetric. 


d Bo 
zajrace (ABA“) = 2AB (2.31) 


Inserting Equation (2.28) into Equation (2.30) and applying Equation (2.31) gives the value 
-20 -GH)P,,, ,H' -2GR = 0. ( 2.32) 
Solving for G, we have 
cu ende Ute AR ( 2.33) 


which is referred to as the Kalman gain matrix. 


G. KALMAN FILTER EQUATIONS 


The set of recursive equations in Table 2.2 provide the time varying optimal gain ma- 


trix and the error analysis of the estimate. 
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TABLE 2.2: KALMAN FILTER EQUATIONS 


Time update (effect of system dynamics) 
Error covariance: 

Prije = 9P,,0  « Q ( 2.34) 
Estimate: 


Measurement update (effect of measurement z ) 


Error covariance: 


Dou ND. ME Ee ( 2.36) 

Residual: 
Cr+] = 2x41- Hry ( 2.37) 

Estimate: 
M e ae | P HR ek SR NE ( 2.38) 


Alternative Measurement Update Eguations: 


G B UM e ( 2.39) 
O (2.40) 
Xakılaşı 7 GE İİ E) (2.41) 


These equations require the following initial conditions, Xq, y and Py o. 
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There are many equivalent formulations for Equations (2-33) and (2-35). Using the 


matrix inversion lemma the latter can be written as 


-1 
Pape 7 Deae en E" LHP, , AR) HP po ( 2.42) 


Note, Equation (2.42) requires only a p x p matrix inversion. Equation (2.36) requires the 
inversion of two v xv matrices, and the number of measurements for p is usually less than 
for v. If |P,, ,,,| = 0, then Equation (2.44) must be used. 

If it 1s often convenient to replace the measurement update equations by alternative 
equations such as those in Table 2.1. 

The Kalman filter gain is the weighting that determines the influence of the residual 
in updating the estimate. It is important to remember that the equations of Table 2.2 are only 


optimal when the system is linear, and when the a priori knowledge of the noise is avail- 


able. 
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III. EXTENDED KALMAN FILTER 


In this section, an approximate solution to the non-linear filtering problem defined 
below is developed. This solution involves the linearization of a non-linear process 
traversing about a reference trajectory and the modification or extension of the linear 
Kalman filter algorithm using the linearized model. 

In practice, many processes are non-linear rather than linear. Coupling non-linearities 
with noisy data makes the signal-processing problem a challenging one. Instead of 
extending this solution to the continuous case, the extended Kalman filter algorithm was 


developed to test for the discrete non-linear system.[Ref. 2] and [Ref. 3]. 


A. SYSTEM DYNAMICS 


Let us discuss the non-linear system of the form 


X,,1 7 Ox, tU, ( 3.1) 


Žk+1 = h (X41) + Ugg ( 3.2) 


where w, ~ (0, Q) and v, - (0, R) are white noise processes uncorrelated with each other. 


B. APPROXIMATE MEASUREMENT UPDATE 


In order to find a measurement update that can be conveniently programmed, A (x, , 1) 


is expanded into a Taylor series about £,, ,,;,, aN @ priori estimate at time K^ I. 
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h (xy, = h Re) 4 9h (x,4.1— £54 14) + higher order terms (3.3) 


X = huh 


Neglecting the higher order terms (H.O.T.) 


h (4,1) = h (dns) +2 O (3.4) 


z X = Šaj 


representing the Jacobian as; 
H(x,k) = Žh(x,k), (3.5) 
dx 


using Equations (3.4) and (3.5) can be used to proceed with the Kalman filter analysis. 


C. APPROXIMATE TIME UPDATE 


To obtain a complete filtering algorithm, update equations that account for measure- 


ment data are needed. We choose a linear, recursive set of equations for the estimate, 


Öke Asi = a, kt Geek > (3.6) 


where the vector a, and the gain matrix G, are to be determined. Proceeding with 


arguments similar to those used in Chapter II, we define the estimation error as 


A A | (19.0 


e eX RI ( 3.8) 
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Equations (3.6) and (3.7) and (3.8) are combined with Equation (3.2) to produce the 


following expression for the estimation error: 


R Oo, G p ek ke 655) 


One required condition is that the estimate is unbiased. Applying this requirement to 


Equation (3.9) and letting E (x,,,,,] = E{v,] = 0, we obtain 


dr wu x rem OO. ( 3.10) 


By defining the residual as the difference between the observation and the expected 


value of the observation in Equation 3.11 


e, 2| 7 Zg 17 A Rap as ČS 


solving Equation (3.10) for a,,,, and substituting the result into Equation (3.6), and 


combining it with Equation (3.11) yields the extended Kalman filter estimate equation 


Yb.lksl A ( 3212) 


D. ESTIMATION ERROR COVARIANCE 


Using the definition of error covariance 


Pers > Eliş e (3.13) 
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and 


- -T 
STE = EB [Xp yee iXes rei) ( 3.14) 


the error-update equations may be derived. Taking Equation (3.10) into account, Equation 


(3.9) becomes 


Xp kel = eşya Gua lh GL) - hu] * GL, (3.13 


Using this, to generate the error covariance P,,, in terms of the yet undetermined 


G,,, we can write 


SE = Pest GE (A (X, ,1) -h(£,4,1] [A (X,, 1) (GV C 
E (x,, y [A (X441) -h(£,,01] C 


GE f [A (X441) -h (SE se 114) = Cr d ( 3.16) 
where 
R, = E{v,v;]. 


The gain G,,, is now selected to minimize P, , ,. Differentiating P,,, with respect 


to G,,, and solving for G,, , results in the desired optimal gain matrix. 
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Er. = El rajo (AG) EE S qx 


{E { a Dİ ACTO CTA ] ris ( 3.17) 


Substituting this into Equation (3.16) and simplifying yields 


DEUM E Fa Greed d [A (x,, 4) Sn S D (3.18) 


The complete linear estimate update due to non-linear measurement is given by Equa- 
tions (3.15), (3.18) and (3.19). However, these equations are impractical to implement be- 
cause they depend on conditional moments of x,, , in computing A (£,,,) . In order to sim- 
plify the computation, we will expand A (x,, ,,,,,) into a power series about £,, ; , as fol- 


lows: 


h(Xy uaa 7 AG e H Ekri) krikt krik) t- CE 


where 


HZ +11) = 2h (x) 


xi, l| k 
Truncating the above series after the first two terms, substituting the resulting approx- 


imation into Equations (3.18), (3.19), and carrying out the indicated expectation operations 


results in a measurement error covariance update as follows: 
[eae MG EDİ aie (20 
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This equation represents an approximate, linear measurement update for the error co- 
variance. The residual is computed using the non-linear measurement function A (x) eval- 
uated about the a priori estimate £,,,,. The error covariance is found using the Jacobian 


matrix. 
TABLE 3.1: EXTENDED KALMAN FILTER EQUATIONS 


System model and measurement model: 


x = f(x,t) +w (t) (5.21) 
zy = h(x(k)) +0, (320) 
w (t) - (0, Q) , v, — (0, R) (3.29) 


Time update: 


Estimate (state prediction): 


PT k = fXy k ( 3.24) 
Jacobians: 
(x,t) = LF (x,t) ( 3.25) 
HG T 2A (x, k) (3.26) 
X 


Error covariance (covariance prediction): 
PIS = 6£,, PP 49 (3.27) 
Measurement update: 
Kalman gain: 
^ A P -1 
Corr = Piu pH oa [H (k+) Prti VA k) +R] ( 3.28) 
Error covariance (covariance correction): 
Palas > [[- G,LíH (34 Pay ( 3.29) 
Estimate (state correction): 


Kami lie = See Gp ea lee eae) ( 3.30) 
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The covariance and gain equations are identical to those in Table 2.2, but with the Ja- 
cobian values A and H linearized about 2, , ,,,. Note that P and G are now functions of the 
current state estimate, which is a conditional mean and therefore a single realization of a 
stochastic process. It also should be realized that the measurement times do not need to be 
equally spaced. The time update is performed over any interval during which no data are 
available. When data become available, a measurement update is performed. This means 
that an indication of missing measurements can be obtained, and pure prediction in the ab- 
sence of data can easily be achieved by using the extended Kalman filter. 

Higher-order approximations to the optimal non-linear updates can also be derived 


by retaining higher-order terms in the Taylor series expansions. 


IV. POLAR MODEL OF THE SYSTEM 


Consider an approximately periodic, non-sinusoidal signal, in additive white 
Gaussian noise. A non-sinusoidal signal may be considered to consist of an infinite number 
of sinusoidal components. Three sets of parameters can characterize the signal: the 
fundamental frequency, the amplitude of each harmonic component, and the phase of each 
harmonic component. 

The signal is not exactly periodic since frequencies, amplitudes and phases change 
slowly over time. We will keep the same notation as defined in Nehorai and Porat [Ref. 6] 
and Parker and Anderson (Ref. 7]. For this purpose, we will first take a periodic signal y (t) 


with a zero d.c. component. A Fourier series representation of this signal can be written as: 


oo 


y(t) = X rsin (kw + 6,) ( 4.1) 


kz1 


In this paper a discrete time domain (i.e. ¢ = 0,1,2,...) rather than a continuous domain 
will be used. As our signal y(t) is not exactly periodic, but has a slowly time varying 


frequency w,, amplitudes r,, and phases 6,, we can state 


Ww, = w,(t) ( 4.2) 
ram O ( 4.3) 
$, = 0, (t) ( 4.4) 
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where w,(t), r,(t) and 9, (t) are nearly constant over a few periods of y(t). In other 


words; 


|w, (t +1) - w,(t)| 


=0 4. 
w(i) (4.5) 
L a 4.6 
"WO ( 4.6) 
6, (£1) — 6, (t 
[NA IO s 9, Mo ( 4.7) 


w(t) 


In the model of y (t), the quantities w(t), r,(t) and $,(f) are the instantaneous 
frequency, amplitudes, and phases of the signal. We will call the model given in Equation 
(4.1) the polar model and the model given in Chapter V the rectangular model [Ref.7]. 

We assume for both models that the signal y(t) is corrupted by noise. The 


measurements are given by 


z(t) = y(t) +u(t). ( 4.8) 


The task is to estimate the values r, (t) ,...,r,, (t) W(t), >, (t) ,... 0, (t) from the mea- 


surements, where m denotes the number of the significant harmonics. Parameters are only 
estimated up to mth harmonics. The higher harmonics are assumed to be negligible. A total 
of 2m+1 parameters must be estimated. 

As explained in Parker and Anderson [Ref.7], we are also estimating amplitudes as 
well as the fundamental frequency. This is required to establish an estimator that uses both 
the energy in the fundamental and the energy in the higher harmonics to estimate the fre- 


quency of the signal. The information about the frequency contained in any harmonic de- 
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pends on the energy in that harmonic. If a particular harmonic component is strong, then 
the estimator of the frequency components should give more weight to the information 
available in the strong harmonic and less weight to the information available in the weak 
harmonics. 

Estimation of the harmonic amplitude also assists in estimating the frequency. The es- 
timator determines the frequency by first estimating the harmonic amplitudes. Knowledge 
of the frequency and the phases in the polar model also assists in the calculation of the har- 


monic amplitudes. 


A. THE ESTIMATOR 


Given a noisy measurement sequence and the associated models as shown in Fig. 
4.1[Ref.4], the Kalman filter can be thought of as an estimator that produces three types of 
outputs. The first filter can be named as a state estimator or reconstructor. It reconstructs 
estimates of the state x(t) from noisy measurements y(t). This kind of model may be 
thought of as the means to implicitly extract x (t) from y (t). 

The second filter, may be thought of as a measurement filter that accepts noisy se- 
quences of input and produces a filtered sequence of output. Finally, the estimator can be 
thought as a whitening filter that accepts noisy correlated measurements and produces un- 
correlated or white residues e (t) = z (t) - 2 (t|t— 1). For our purposes, we will concentrate 


on the measurement filter form. 
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y(t) State x (t| t) 
estimator 

y(t) Measurement $t) 
filter 

y(t) Whitening e (t) 
filter 


Figure 4.1 Various representations of the Kalman filter 


As mentioned before, the extended Kalman filter will be used to estimate frequency, 
phases, and amplitudes of an almost periodic signal imbedded in noise. First, we need a 


state space representation of the signal defined in Equation (4.1). Therefore, 


x(t* 1) 2 ox(t) *w(t) ( 4.9) 
Zit) = sat) tv) ( 4.10) 
ZL) CXL) yb) ( 4.11) 
and 
UE (6207 (00 AO (O O (E) jE (412) 
and 
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yl 
$-|010 ( 4.13) 
0 01, 
where, J,, is a mth order identity matrix, 
h(x(t)) = 5 r, (t) sin (kugt + $,) ( 4.14) 
pou 
and w (t) is white Gaussian noise, with a zero mean and a variance 
E [w (t) w! (t)] = Q. ( 4.15) 


The observation noise v (t) is also white Gaussian noise, with zero mean and has a 


variance 


E(v(t)v  (0] - R ( 4.16) 
( 4.17) 


and is uncorrelated with w (t). 


E [w (t)v (t)] 2 0 ( 4.18) 


We will have a Q matrix which is diagonal. Its value will be given in the simulation 
section. From Equation (4.9), it can be concluded that the harmonic amplitudes evolve 
randomly over time. Also, the same argument is true for w, (t) , the fundamental frequency, 


and the 6, (4) phases of the signal. The rate of the random walk will be determined by the 
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diagonal Q matrix. A zero Q matrix will correspond to constant amplitude, frequency and 
phase. In order to estimate x (7| 2) or x (z| t- 1) of x (t) from the measurement z (t) , the ex- 
tended Kalman filter will be applied. Here x (:|?) denotes the estimation of x(t), given 
measurements (z (1)|1 = 0,1,2......f) up to and including time r. The value x (t|t— 1) is an 


estimate of x(t), given z(t) up to time r-]. 


K(t|t) = X(t/t-1) + G(t) (z(t) -h(x(tlt-1)] ( 4.19) 
£(t-Mt) = 02 (11) ( 4.20) 

G(t) = P(t)H7 (t) (H(t)P(t)H’ (t) +R) ( 4.21) 
P(t+1) = O[P(t) -G(t)H(t)P(t)]}07+Q ( 4.22) 


where H(t) is the Jacobian of A(t). That is: 


dh (å (t|t-1)) 


Hitt) = 
id ox (t| t — 1) 


sin (b, (tt — 1)t € $, Q|t- 1)) 


sin (b, (t|t— 1)t* $9 Q|t- 1)) 


sin (W,(t|t- 1)t+0,(t|t- 1)) 
Ht) al ( 4.23) 


F,(t|t- 1)cos (by (t| t— 1)t 6, (tt — 1) 


P, (t|t — 1) cos (ig (t| t— 1) t $5 (| t— 1)) 


Fm (t|t— 1) cos (Wp(t|t- 1)t+0;(t|t-1)) 
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where, 


4 
S(|t-1) — } Pytcos (wy (t|t- 1) t $, (1 £— 1)) 
k=] 


and the initial values are 


£(0) = Elx(0)] = x(0) 


and 


P(0) = E[(x(0) -x(0)) (x (0) -x(0))7]. 


B. SIMULATIONS 


In the simulation we let 


z(t) 7 r,sin (210.04t * 0.01) *r,sin (410.041 + 0.01) +r, sin (670.04t + 0.01) + 


r, (810.04t + 0.01) +v (t) ( 4.24) 


where v(t) 1S a zero mean, white Gaussian noise process with a variance of 0.1. The 
fundamental frequency of the signal w, - 210.04 and the amplitudes of the signal r, are 


constant over time. We use the amplitudes as written below. 


k = 2,3,4, ( 4.25) 
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where r, is chosen for a desired signal to noise ratio as given by 


For the simulations, the filter program was written in MATLAB (See Appendix A), 


with the following initial conditions: 


4 


SNR - 10 log EIE 


( 4.26) 


F,(0) = 2.4 (4.27) 
FP2(0) = 0 ( 4.28) 

F,(0) 20 ( 4.29) 

F,(0) 20 ( 4.30) 

W,(0) = 210.03 (4.31) 

$,(0) 20 ( 4.32) 

$,(0) = 0 (4.33) 

$,(0) 20 ( 4.34) 

$,(0) 20 ( 4.35) 

P(0) = diag {6,5,3,2,0.08,0.01,0.01,0.01,0.01} ( 4.36) 


As stated in Candy [Ref. 4], the variation of the gain 1s related to the variations in P 
and Q. Uncertainty in the model can be characterized by the process noise covariance, Q. 
For a large Q, P is large, which indicates a high uncertainty or an inadequate model. For 
small Q, P is small, indicating an adequate model. Therefore, the Kalman gain can be 


thought of as a ratio of process to measurement noise; that is, 
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Go 


vd 


( 4.37) 


It can be seen that the variations in G are proportional to the variations of Q. The Kal- 
man estimator can be perceived as a deterministic filter with a time-varying bandwidth as 
determined by the filter gain. As Q increases, both G and the filter bandwidth increase. 
Thus, the filter transient performance will be faster. The same effect can be observed by a 
small R. Conversely, if Q decreases, G decreases, which decreases the bandwidth of the fil- 
ter, and hence, the filter transient response is slower. 

The steady-state value of the error covariance Wa P increases (decreases) with a 
corresponding increasing (decreasing) value of the Q matrix. We conclude that P is also 
proportional to Q. We also note a similar effect by varying R. 

We investigated the effect of changing the initial conditions of P on the algorithm per- 
formance. As long as the filter is stable and the state-space model is completely controllable 


and observable, then 


limP (t) = P (small). ( 4.38) 


Thus, an estimator error approaching zero implies that the state estimate converges to 
a true value, given that enough data exists. The initial estimates will affect the transient per- 
formance of the algorithm, since a large initial P(0) | gives a large G (0), and therefore 
heavily weights the initial measurements and ignores the model. 


In the program, we used R = 0.1, while 


Q -—udiaetqougtonmbus0295.02:0» 1 ( 4.39) 
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where 


qı = 1x10% and q, = 1x107. 


The entries of Q and R roughly determine the bandwidth of the estimator. This also 
affects the capture properties, tracking properties, and steady-state error. The capture prop- 
erty can also be influenced by the choice of P (0) . The values used in this work were largely 
obtained by trial and error. 

Figure 4.6 shows the true and estimated values of the fundamental frequency from 
450 samples. The estimated value of the frequency tracks the true value after ¢ = 30 and 
locks onto it very closely. Figure 4.2 shows the true and the estimated values of the ampli- 
tude of the first harmonic component. Tracking is achieved after t = 50. Figure 4.3 shows 
the estimated and the true amplitudes of the second harmonic component. Tracking is 
achieved after r = 50, although the initial estimate value F, (0) is zero. The other harmonic 
amplitudes and their estimates show similar responses, as in Figures 4.4 and 4.5. 

The phase error of the fundamental component is shown in Figure 4.7. The error is 
close to zero after ¢ = 350. A similar response can be seen in Figure 4.8 for the phase error 
of the second harmonic. Figure 4.9 shows the phase error of the third harmonic. Att = 450, 
the error is 0.00025. Figure 4.10 shows the phase error of the fourth harmonic, which has a 
steady state error of 0.007 after ¢ = 250. The phase error of the second and the higher har- 
monics is higher than in the first harmonic. This is probably because the initial estimate of 
the second and the higher harmonic amplitudes were zero. 

Since higher harmonics are assumed to be negligible we only considered tracking sig- 
nals with less than four harmonics. For one case, we accepted a signal consisting of two 


harmonics. For the simulations, we let 
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z(t) = r,sin (2n0.04t + 0.01) +r,sin (610.041 + 0.01) tv (t) ( 4.42) 


and the second and the fourth harmonics are assumed to be missing. The filter could not 
initially track the initial error covariance matrix used with four harmonics. So we 


rearranged the initial value of the error covariance matrix as 


P(0) - diagt2,.1:0:5.0:250:0450:01:0:001:0:015:0 0] 5 ( 4.43) 


We used the MATLAB program in Appendix B for the simulations. The results are 
given in Figures 4.11 to Figure 4.15. Figures 4.11 and 4.12 show the true and estimated 
values of the first and the third harmonics, respectively. For both cases tracking is achieved 
after t = 30. The second and the fourth harmonic true and estimated values were around 
zero. These graphs are not included. Figure 4.13 shows the frequency tracking of the signal. 
Its transient response was similar to the one obtained for the case of four harmonics. Figures 
4.14 and 4.15 show the phase error of the first and the third harmonics, respectively. The 
phase error of the third harmonic was worse than the first harmonic because of the poor ini- 


tial value of the third harmonic. 
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Figure 4. 2 True and the estimate value of the amplitude of the first harmonic 
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Figure 4. 3 True and the estimate value of the amplitude of the second harmonic 
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Figure 4.4 True and the estimate value of the amplitude of the third harmonic 
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Figure 4. 5 True and the estimate value of the amplitude of the fourth harmonic 
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Figure 4. 6 True and the estimate value of the fundamental frequency 
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Figure 4. 8 Phase error of the second harmonic 
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Figure 4. 9 Phase error of the third harmonic 
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Figure 4. 10. Phase error of the fourth harmonic 
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Figure 4. 11 Amplitude tracking of the first harmonic. (Two harmonic case) 
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Figure 4. 
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Figure 4. 13 Frequency tracking. (Two harmonic case) 
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Figure 4.14 Phase error of the first harmonic. (Two harmonic case) 
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Figure 4. 15 Phase error of the third harmonic. (Two harmonic case) 
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V. RECTANGULAR MODEL OF THE SYSTEM 


This chapter applies the extended Kalman filter to the signal formulated in Chapter IV, 
but represents it in a different way. The signal is not periodic but has frequency, amplitude 
and phases that change slowly over time in a similar way to that in Chapter IV. Three sets 
of parameters (frequency, amplitude, and phase) define the signal. We will call the 
parameterization in this chapter the rectangular model as defined by Anderson and Parker 
(Ref. 7] and Anderson and James [Ref. 8]. In this representation y(t) has amplitudes and 


phases with sine and cosine components, like 


Co 


y(t) = a,(t) sin (w,(t)t+(t)) + M [a, (t) sin (k (w-(t)t+0(D))] + 
k=2 


> [b, (£) cos (k (wp (t) t 6(0))] (o) 


k=2 


le parameters of the system are a, (t), a, (¢)...44,, (1), 65 (t) ,.....0,, (¢),,w,(),, and 


(t). The signal y(t) has slow time varying frequency, amplitudes, and phases. That is: 


wp = wet) (5.2) 
dv = alt) (>) 
b, = b, (t) (5.4) 
d= o(t) (3.5) 


where frequency, amplitude, and phase are nearly constant over several cycles. 


|wr(t +1) - w,(t)| 


d (5.6) 
W(t) 

b 1)-5 

| p(t + ) BOI ( 5.8) 
w(t) 

let *D -e(0l 9 ( 5.9) 


W(t) 


In the representation of the signal, we did not include the 6, (t) term cosinusoidal 
component of the fundamental frequency. This is because such a cosinusoidal frequency 
would give a phase shift in the fundamental, represented by a change in the phase of the 
fundamental. 

Regardless of the working model, we will assume that the signal is contaminated by 
additive noise. This noise v (t) will be white and Gaussian, which gives the measurement 


as: 


z(t) = y(t) +v (t) ( 5.10) 


From these measurements, we will estimate a,(t), as5(t), ..,a, (t), b;(t), 
-bm (t) (8), 6 (8). The estimation will be carried out up to the mth harmonic. The higher 


harmonics will be assumed to be insignificant for our purposes. 


A. THE ESTIMATOR 


The extended Kalman filter will be applied for the estimation of frequency, amplitude, 
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and phase of the measurement signal. In a similar way, we will denote the state-space 
model of the rectangular form. Therefore, Equations (4.9) and (4.11) remain unchanged, 


while the others become 


x(t) = [a,(t) a (t),...,4,, (t) b) b (EY we (0,0 (017 ( 5.11) 
In 0 00 

De m O (5.12) 
00 10 
00  01| 


h(x(t)) = a, (t) sin ((w,(t)1+0(t))) + > [a, (£) sin (k (w,(t)t+(t)))] + 


k=2 


Y [b, (t) cos (k(w,(1) +9 (0) )) ] (5.13) 
5 


The estimator Equations (4.18), (4.19), (4.20) and (4.21) of the polar form remain the 
same as for the rectangular form. The only difference appears in a Jacobian matrix as writ- 


ten below: 


sin ( (P; (|t — 1) & 6 (tt — 1)) 
sin (2 (i (t| t — 1): ó(t|t — 1)) ) 


sin (m (i, (|t — 1)t- 6 (t| t— 1)) ) 
Hay = n | ( 5.14) 
cos (2 Que t— Dico 19) 


cos (m (ip (1t — 1) t 6 (| £— 1)) ) 
SGT) 
go (t|t—1) 
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where, 


S(tt-1)- Y kà, (t| t— 1) tcos (& (y (tt - 1) t 6 | £— 1)) ) = 


k=1 


Y kb, (|t - 1) tsin (& (p (tt - 1)t 9 (t£ 1))) 
k=2 
and 


o Y kà, (t t— 1) cos (k (op (|t - 1) t (tlt -1))) : 


k=1 


Y kby(t|t- 1) sin (k (y (t|t — 1)t  $(t|t- 1))). 


k=2 


B. SIMULATIONS 
Like the polar model, the rectangular model also has 27m 1 states, with m representing 
the number of harmonics. We choose the number of harmonics m=4 to compare with the 


polar model. The measurements consisting of sine and cosine components are 
z(t) = a, (t) sin ((27.0.04t + 0.01) ) +a, (¢) sin (2 (270.041 + 0.01) ) + 
a, (t) sin (3 (270.041 + 0.01) ) +a, (t) sin (4 (270.041 + 0.01) ) + 
b, (t) cos (2 (210.04t + 0.01) ) +b, (t) cos (3 (27. 0.041 + 0.01) ) + 
b, (t) cos (4 (270.04t + 0.01) ) +v (t). (315) 


Here v (t) is zero mean, white Gaussian noise with a variance of 0.1. The signal has 


4 


a fundamental frequency wp = 210.04 and amplitudes of a, and b, for the sine and the co- 


sine components, respectively. They are defined as 





e 5 e a (5.16) 
b A, ae 
dox 42004 (517) 





The values of the amplitudes can be obtained from the desired signal to noise ratio. 
The program written in MATLAB (See Appendix C) is used for the simulations with the 


following initial conditions: 


di — 124 ( 5.18) 

as 0 (5.19) 

—-0 (5.20) 

â,-0 (5.21) 

b, < 1.45 (05:20) 

b, = 0 ( 5.23) 

b= 0 ( 5.24) 

W = 210.03 (525) 

>= 0 ( 5.26) 

P(0) = diag {1, 0.5, 0.1, 0.04, 0.5, 0.1, 0.04, 0.8, 0.01} (327) 


These initial values are chosen closely to those used in the polar form. The main dif- 
ference appears in the initial value of the error covariance matrix. The program uses the 


same value of measurement and process noise as in the polar form. 
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Figure 5.1 shows the true and estimated values of the amplitude of the first harmonic. 
Tracking is achieved after ¢ = 50. In Figures 5.2 and 5.3, the amplitudes of the sine com- 
ponent of the second and third harmonics show a similar response to the first one. Figure 
5 4 shows the true and estimated values of the sine component of the fourth harmonic. The 
error looks worse than the others with the filter tracking after ¢ = 150. Figures 5.5, 5.6 and 
5.7 show the estimated cosine components. Their responses are similar to the sine compo- 
nents. Figure 5.8 shows the frequency tracking of the signal. The peak value of the estima- 
tion is large compared to the one estimated in the polar form. But, like the polar form, the 
rectangular model tracks the frequency after t = 30 and locks onto it very closely. Figure 
5.9 shows the phase error of the signal. It has a steady state value of —0.0003 after ¢ = 70. 


We also applied a case with less than four harmonics to the rectangular model. The 


measurements are assumed to be 


z(t) = a,(t) sin (270.04t + 0.01) +a, (t) sin (3 (270.04t + 0.01)) + 


b, (t) cos (2 (270.04t + 0.01) ) tv (0). ( 5.28) 


The second and the fourth harmonics of the sine component and the third and t 
fourth harmonics of the cosine component are assumed to be missing for the rectangu 


model. Similar to the polar form, we also rearranged the error covariance matrix for t 


rectangular form as 
P(0) = diag {1, 0.2, 0.1, 0.02, 1, 0.1, 0.01, 0.1, 0.001}. ( 5.29) 


The MATLAB program used for the simulations is shown in Appendix D. Figure 5.1 
shows the true and estimated values of the amplitude of the first harmonic. Its response i 


almost similar to the one estimated with four harmonics in the rectangular model. Figu 
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5.11 gives the estimation of the amplitude of the sine component of the third harmonic. Fig- 
ure 5.12 shows the amplitude tracking of the cosine component of the second harmonic, 
which is very similar to the one obtained in the four harmonics case. The true and estimated 
values of the second and the fourth amplitude harmonics for the sine component were 
around zero. The same results apply to the third and the fourth amplitude harmonics of the 
cosine component. These graphs were not included. Figures 5.13 and Figure 5.14 show the 


frequency tracking and the phase error of the signal, respectively. 
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Figure 5.1 True and estimate value of the amplitude. First harmonic 
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Figure 5.2 True and estimate value of the amplitude. Sine component, second harmonic 
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Figure 5.4 True and estimate value of the amplitude. Sine component, fourth harmonic 
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Figure 5.5 True and estimate value of the amplitude: Cosine component, second harmoni 
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Figure 5.6 True and estimate value of the amplitude: Cosine component, third harmonic 
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Figure 5.7 True and estimate value of the amplitude. Cosine component, fourth harmonic 
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Figure 5.8 True and estimate value of the fundamental frequency 


0.12 
olp 
O0RE 
0.06 


0.04- 


Phase error 


-0.02 


-0.04 





0 50 100 150 200 250 300 350 400 450 


Time 


Figure 5.9 Phase error of the signal 
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Figure 5.10 Amplitude tracking of the first harmonic. (Three harmonic case) 


Amplituse 


0.9 


0.8F 


0.7 


0.6L- : 


0.5 


0.4 


0:3 F- 





ps EE D T T 
T JE t e zh e 

a if y y x .. i v r, 

ina vE í ' M paws! : ee 

ta 4 1 y , " n ` , 2 a 

mu "^ 2 e : S 

1 v 1 a 

I 1 7 

| : 

1 
¿True Sute --Estimate of the State 

50 100 150 200 250 300 350 400 450 
Time 


Figure 5.11 Amplitude tracking. Sine component, third harmonic.(Three harmonic case) 
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Figure 5.12 Amplitude tracking. Cosine component, second harmonic. 
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Figure 5.13 Frequency tracking. (Three harmonic case) 
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Figure 5.14 Phase error. (Three harmonic case) 
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VI. CONCLUSIONS 


This thesis presents the estimation of frequency, amplitude, and phase of a signal using 
an extended Kalman filter algorithm. Both the polar and the rectangular model filter were 
implemented. In both models, the signal is contaminated by noise. 

The simulations show that the performance of both filters are similar. Although there 
is little difference between the two models, the rectangular model seems to be able to 
estimate phase better than the polar model. For both filters, the tracking of the fundamental 
frequency plays a significant role. If the estimated value of the frequency locks onto a 
multiple of the true frequency, the filter cannot track the amplitudes. Each model offers a 
different advantage. The estimation of amplitudes of the signal is more convenient to 
measure in the polar model than in the rectangular model. 

In all the simulations, an 18 dB signal-to-noise ratio was used. The performance of the 
two filters was poor when lower signal-to-noise ratios were used. The signals may have a 
Doppler shift. By adding another state, additional Doppler effects can be accounted for in 


future work. 
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APPENDIX A 


$This program tracks the frequency, amplitude, and the 
phase of the nonperiodic signal in noise by using extended 
ir eman filter algorithm for the polar model. 


$ State space model 


pL 0:0 0.00 0 0 0 
Li 0 UYU YU 
EÇ YU) 
ww Ul O 20 0570 
O UY LU 9 09: 0.0 
22080 Oa) - Xr creo 
Eo 00070. 0. 1000 
EE 0:70 D S0 9 20 
uo c0 2D O0 0 Oi; 


au tiadl conditions of the true state 
EM SAmplitude of the first component 
B E (1)/2; SAmplitude of the second component 
E 1)/2; SAmplitude of the third component 


5 E 
< P 3(1)/2))shimpiitude of the fourth component 
ob) =0. 017" SPhase of the first. component 
E 01; $Phase of the second component 
3(1)=0.01; %Phase of the third component 
e Ole wePnase OL the fourth component 
w(1)22*pi1*0.04; SFundametal frequency 


zuu)tral condition of the estimation 
DES 1)9[12.6;0;07D022*p1*D0.05,0707070]7 


R=0.1; $ measurement noise 
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gisle >; <5 


g2-le-7; 

q3-1e-7; 

O=[gl 0 0 O 
Care ais 
OO gli g 
00 "Omen 
DORO- © 
Gi 
0000 
Da) 
OVO 


process noise 


Oro (C158 


CSC 


Coco Gn 


a C») © O © 
CO ee CO CEC 
oC © O <> © 


9 


ORO cou © Oo © 


q 
Duc 
0 0 


O 


SEO Goo m m rm 


0 


CONO CO (co CES 

ONO OTORO ORDIN S 
COME Oe AC "C 
OOTO Cee Vee cC» 


0 


COMICO O SN RO © 


$Jacobian matrix 
e [5 mM OO 


$Transpose of the Jacobian matrix 
Rieter GS 


t=1:450; SNumber of samples 


$STrue state matrix 
p yir b 


162 CUL Y 
Sele) 
r4 (1) 
w (1) 

o) 
OZ (1) 
e 
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pay Js 


Me) — 45) 
tr mai normal”) 


$Observation 

Zzam 2. 1) *sin(x(5,1)*1i+x(6,1)) + 
Si (27x (5,1) 21 rx LEA SAS An SAS AS, LE 
a FA sin(1*X(5,1)*1+x(9,1))+0,1*rana (1) ; 


be =xkkm(l,1i)*sin(xkkm(5b,1)*i+xkkm(6,1))+xkkm(2,1)*sin(2*x 
DE ip iitxkkm(/, uy) xkkm( 35,1) *sin(xkkma(b5,1) xS*ıtskkm(8,1)) 
zn mii) *sın(xkkm(5,ı) #axıtxkkm(9;1)); 


5 Kalman gain 
e li anv (H(i S) PHI: Rİ; 


$ Estimate update 
x 1) —=xkkm(©,1)+L(:,1)*(z(i) = heQ)); 
sblení:,1+1)=F*xkk(:,1) 


$Tranpose of the Jacobian matrix 

Be m url) —|sin(xkkm(5,1)*1irxkkm(6,1)); 
YEN “xkkm(5, 1) 'ıtxkkm(7/;1)); 
Simio, 1)*3*1+xkkm(8,1)): 

sa kkm(5,1)*4*1+xkkm(9,1)): 


xkkm(1,1)*1i*cos(xkkm(5,1)*i+xkkm(6,i))+xkkm(2,1)*2*i*cos(2*x 
m > t) ~1+xkkm{( 7,1) ).. 

eens 1) Oe COS (3 ekki (oO, 1.) “ax Kee) ) teem 4 1) 421006 
TT >kkm(5,i) *itxkkm(9,1)); 

e Ii) *cos(xkim(5, ij) ~14+xkkm (6; 1) 


EM 1)*cos(2*xkkm(5,1)*r-oxkkm(7,1)); 
ENS | UN Io 
mem (4, 1) *cos (4* xkkm(5, 2) * ol EUM ES 
% Jacobian 


(e, tl)” 


$ Error covarlance update 
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PE (BU) FG LE 


v(:,1)=[rand(1) *le-2 
rand(1)*1e-2 


rand(1)*le-2 
bane (ar we 
rand (lj |(e-4 
rana) * 64 
rand(1) *1e-4 
rana (1) *1e-4 
rang e il; 


$State update 

x (274141) =F *x (25.1). ee eee 

end 

plot (t;x(1,;1:1) 7, kk Oe ari abel NEUEM 
vlabel (*Amplitude“ ),gtext (*-True State" ),gtext(*- 51s 
of the State“ ),meta tezlal 


plot (t,x(2,1:1),t,xkk(2,1:1)) pause, grFEid7xLabel te 
ylabel (‘Amplitude’),gtext(‘-True State’),gtext (‘--Estimate 
of the State'),meta tezla2 


plot (t,x(3,1:1),t,xkk(3,1:1));pause,orid, xlabel (A 
ylabel('Amplitude'),gtext('-True State'),qtext('--Estimate 
of the State’),meta tezla3 


plot (t,x14,1:1),t,xkk(4,1:1)) pause, grid, xlabel ( ‘Tinea 
ylabel(‘Amplitude’),gtext(‘-True State’),gtext (‘--Estimate 
of the State’),meta tezla4 


plot (t,x(5,1:1),t,xKkK(S,1:1) ) ;pause, grid, xlabel (Tiina ae 
ylabel (‘Frequency’), gtext(‘-True State’), gtext (‘--EsStimate 
of the State’),meta tezla5 


plot(t,x(6,1:1)-99 06) bate ise ge 
xlabel(‘Time’),ylabel(‘Phase Error’),meta tezla6 


plot (©; x(;141) x&k (a, Ls pause ake 
xlabel(‘Time’),ylabel(‘Phase Eror’),meta tezla7 
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DE de, x(o;, 1 a) -ekk(8,1s1)) ;pause,grld; 
xlabel (*Time“ ),ylabel (*Phase Error'),meta tezla8 


eee 2 (0 0l.1)-xkk(9, 1:2) pause, grid. 
xlabel (‘Time’), ylabel(‘Phase Error’),meta tezla9 
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APPENDIX B 


$This program computes the frequency, amplitude and the 
phase of the,nonperiodic signal in noise which consists of 
two harmonics for the polar model.It uses extended Kalman 
filter algorithm: 


% State space model 


Po L LOO O SONDA 
DO 000009 
BO -10700009 
OA OO ee Oar) 
070700 0000 
O0 O0 Oo 00 
O VU MOON Um Le RD 
05:0 050707000 
001070000 Oe 


% Initial conditions of the true state 

rl (1)=2.8; %Amplitude of the first component 
r2(1)20;$r1(1)/2; %Amplitude of the second component 
r3(1)2r1(1)/4; %Amplitude of the third component 
r4(1)=0;%r3(1)/2; SAmplitude of the fourth component 
O1(1)=0.01; $Phase of the first component 
O2(1)=0;50.01; %Phase of the second component 
O3(1)=0.01; %Phase of the third component 

04 (1)=0;%30.01; SPhase of the fourth component 
w(1)=2*p1*0.04; SFundametal frequency 


$Initial condition of the estimation 
xkkmt:,19 02.650590. 2 515 0890 90/2 > 0 DDD 


R-0.1; $ measurement noise 
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el (5; $ process noise 
q2=1le-7; 
q3=le-7; 

(al 0000 

Wa V000 

ORO 

1 0 


CCD 
OTO O C9 C) c 
See Pe C 


3 


LOTO O lo O 
SOLO OPM MOTORS © 
eee eğe ie 

Cou ee o 

SS S CSA 

SHS oD 

ČES O OMSK 


q 
US 
0 0 E 


„O 


Error Geovarlance matrix 


E- 2 0 0.000 0 0 0 
del 000070010 
OY > UY YU UY 
VU Oz YU 00 
EU O UYU YAĞ 0 5000 
CU OO OO. OYLYU O 
IO 000 0001700 
2250-20 0070710020000 
UE 0 9790-0 :9:50-:0: c0 DO Y des 


la oblan matrix 
e -U 0 0.0000 U 01; 


STranspose of the Jacobian matrix 
mu 1) =H (1,2) > 


el 450; 


True state matrix 
Al) -(rlii(l) 
rel) 

nr) 

r4 (1) 

w (1) 

O11) 

OZ T) 


6l 


OS) 
OE 


MOB L — M Ja 
rand (“normal”) 


%Observation 

Z(x)ex(l3)*sun(x(5 32 X DEDE: 
x(2,1)*sin(2*x(5,1)*i+x(7,1))+x (3,1) *sin(3*x(5, 1) Ir O PEED 
x(4,1)*sin(4*x15,1)"1 xe brama 


he (1) =xkkm (1,i)*sin(xkkm(5,1)*1+xkkm(6,1))+xkkm(2,1)*sin(2*x 
kkm(5,i) *itxkkm(7,i1)) -xkkm(3,1)*sin(xkkm(5,1i1)*3*i-c-xkkIW PENA 
+xkkmC4 1) * Sim (kho dlc EDS 


$ Kalman gain 
Eli TE O, A ERE 


$ Estimate update 
xkk{:,1)=-xkkm(: EE D MCI E ES 
xkkm(:,i+l)=F*xkk(:,i); 


$Tranpose of the Jacobian matrix 
Http) Sin A emo 4) 
sin (2*xkkm (9 e ek m ee 

sım (<kkıj oy erk Or 
sin(xkkm(5,1)*4*1+xkkm(9,1)); 


xkkm(1,1)*1i*cos (xkkm(5,1)*1+xkkm(6,1))+xkkm(2,1)*2*1 "COMES 
KMS RO A P 
+xkkm(3,i)*3*i*cos (3*xkkm(5,i)*i+xkkm(8,i))+xkkm(4,i)*4*i*co 
S(4*xkkm(5,1)*itrxkkm An 
xkkm(171) "cos Coke cT Sm N; 
xkkm(2,1)*cos (2* xk mn (7, 13998 
xkkm (3,1) *COS (3+ moreno rams a n\n 
xkkm(4, 1) *cos(4*xkkm(5, 1) iTxkkm(9,1)) l: 


%Jacobian 
A(1+1,:)=Ht (:,1+1)'; 
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*$ Error covariance update 
o (PL Hi P) AFO; 


v(:,1)=(rand(1)*le-2 
wane (1) *]e—Z 
noandoly^41]e-2 
pand(l)*le-2 
tana (1) * le=4 
rand(1)*1e-4 
rand(l)*le-4 
Bamc (l) *le-a 
pand(l)*1le-4]; 

$State update 

pac LI)-E*e(: 01) FU 

end 

ENS SUE 0 L:tYPptXxKKk(l,l:1));pause,grid,xlabel( Time^)J, 
ylabel('Amplitude'),gtext('-True State'),gtext("'--Estimate 
of the State’),meta tezlbl 


eerie x2, 1) ta xkk (2,131) ); pause, grid, xlabel{ ‘Time’ ), 
ylabel( ‘Amplitude’),gtext(‘-True State’),gtext(‘--Estimate 
of the State’),meta tezlb2 


Pee t,x (3, 121),e,xkk (3, 1:1)) ppause,grid, xlabel (‘Time’), 
Veli Amplitude’ ),qtext (‘=-True State’), gtext (‘=--Bstimate 
of the State’),meta tezlb3 


jee (c, x (4,1 71),t,xXKK (471:2)) -pause, grid, xlabel (‘Time’), 
vaca ( Amplitude’); qtext (*-Trie State’), gtext(*--Bstimate 
of the State'),meta tezlb4 


NE c (5. 152); EXER (5, lel) bs pause,grid, xlabel (“Time”), 
vlabel (*Amplitude“ ),gtext (*-True State'),gtext('--Estimate 
of the State“ ),meta tezlb5 

Pot t.x(6 1:1)—SKkk(06,122))2pause,grid,xlabel('^Time"); 
ylabel('Amplitude'),gtext('-True State'),gtext('--Estimate 
of the State'),meta tezlb6 


picture iu ds 1) pause grig.zlabel( Time”) 
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ylabel('Amplitude'),gtext('-True State’), gtext ( ‘--Estimanre 
of the State’),meta tezlb/ 


plot (t;x(8,1l:i)-xkk(8,1:i));/paúse, grid, xlabel( TIn EE 
ylabel (‘Amplitude’),gtext(‘-True State’),gtext (‘--Estimate 
of the State'/),meta tezlb8 


plot (t,x (9, 1:1) -xkk (9,1:1) ); pause, grid, xlabel( 1 ime 


ylabel (‘Amplitude’), gtext(‘-True State’), gtext ( v— -ESTEI = 
of the State’),meta tezlb9 


% 
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APPENDIX C 


% This program trackes the frequency, amplitude, and the phase of the nonperiodic 
signal in noise by using extended Kalman filter algorithm for the rectangular model. 


% State space model 


E11 000000 0 0 
YU MU UY 0.0 
OTL O 0 0:0 0.0 
3S0 0 1 90 0 00 -0 
2S0 CO 1 099 70-0 
WU YU LU U 0 
OOO 0.0001 00 
wv 0 000 1-0 
RU UY UY); 


% Initial conditions of the true state 
a(1)=2.7;%Amplitude of the first component. 
a2(1)=a(1)/2;%Amplitude of the second component. 
vr 1)—a(2) /2; tAmplitude of the third component. 
a4(1)=a(3)/2;%Amplitude of the fourth component. 
w(1)=2*p1*0.04;%Fundamental frequency. 
01(1)=0.01;%Phase of the first component 
b2 (1)=a(1)/2;%Phase of the second component 
p3(1)-a(2)/2;$Phase of the third component 
b4 (1)=a(2)/2;%Phase of the fourth component 


Initial condition of the estimation 
EX: 12499905054. 45050505 7p])390€0559]5 


R=0.1; € Measurement noise 
q1-1e-3; $ process noise 


q2=le-7; 
q3-1e-7; 
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La 

EEG 
EG C 
ORORO 
OO 


re 
DAD ID 


= 
O TOTO O 
ORG (c 


1 


CO Nw O CO) © ©, S48 
Oi CO AFS 1 S S40 
S (©. O © 07019 
OTO O “OO 19 

> OTORO 

OS OQ 

GO XGD OORO O OOTO 


q 
02 
0 0 JE 


O 


CEYrOF covarrance matrix. 


Pkk—|1 0 0 020050 0 00 
OVO g0 Gene Oi) 
OOO 2900) 0 00 (UU 
OYU YA OO OMO 
0007070050000 
UU 30:50 70 009 
O YY ao ye Yad 
0707070070000 
0 20707 970 000 OO E 


%Jacobian matrik. 
H(1,:)-—[0- 0 007.00 


$Transpose of the Jacobian matrix. 
Ht(:,1)-1[0 00905070 UOC A 


t=1:450;%Number of samples 


$True state matrix 
xs: lp al) 
a2 (1) 
a3(1) 
a4(1) 
D2) 
DSL) 
b4 (1) 
w (1) 
oC) 
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evı 50 
rand(‘normal’ ) 


Xx —.17)*S$92n(0x(8,1)*14x(9,3))4Tx(2,1)*s1n(2*(x(8,1)*1*x(9, 
Mme 1)5*spxm(9$*(09(9,1) *135-x(9;1)9)-*x(4. 2) *s2n(4*(y*9 B ji 
PCr <i o1) “cos (2 (x (8; 1) *1+xk 1911) )) +x (0,1) “eos (3* (18,1 
Mateo, 1.)) ) Fx (7,1) *ecos (4% (x (8,1) *1+x% (9, 1) ))4+0-1*rand(1); 


$Variable 

ie - kem L, 1)*sin(xkkm(8, 1) *itxkkm(9,1)) rxkkm(2,1) *sime2* 
Em 1) 1+4xkkm(9,1))) rPxkkm(3,1)*sin(3*(xkkm(8,1)*i+xkkm(9, 
peer Kkm(d 32)*sin(4^(i*xkkm(8,1) txkkm(9,1))); 


$Variable 
| EE --kkmi5,1)*cos(2*(xkkm(B,1)*1r-xkkm(9,12))) *xkkmí6,1)*cos 
m km0o8;1)*1iT9*xkkm(9.1))) *xkkm(7,1)*cos(4* (xkkm(98,1) ^iTXkk 
m jj; 


me) =je(i)rtke(i); 


$Time update of the error covariance 
Em E SPRK*E" +0; 


$ Kalman gain 
PES -Pkkm H(1,:)'*2nvtH(il,T) PKM" H(I ey +R) 


$ Estimate update 
a iy xkkm(e,i)iK(: 1) (a (a) che(1i)); 
km: ,irl)er*xkk(:,ı); 


$Variable 

Gr km(l 1) <i*cos(xkkm(8, 1) 1ixkkm(9 17) exkkim( 2, a) “271 co 
S -EkmiB) 1) *usxkkm(9, 1) )) -FXkkmio 1)? 352*cos(3 V (xkkmi(s, T) 
km Of 1) 1) exkkm 4,1) 41 costa 1m<kkm(8, 1 r2kkm(9, 1) 


$Variable 

re —- xs kkin(o, 1) 42> samt 2* (xkkm (8, 1) “1 4xkkmi 9,2) ) ) = 
mo) > xi“sin(s5*(<kkm(8,1)  tikkkm(9,1)))- 

eml 2)vd*a sq nid tiom UB 01) 3s xkkmt9, 2) 5 
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$Variable 

f (1) =xkkm(1,1)*cos (xkkm(8,1) *i+xkkm(9,1)) +xkkm(2,1) “2 GS 
(xkkm (8,1) *1+xkkm(9,1)))+xkkm(3,1)*3*cos(3* (xkkm(8, 1) ^a M 
(9,1) ))+xkkm(4, 1) *4*ce@s 4 sae O cn E 
g(i)=-xkkmw(5,1)*2*sin(2* (xkkm(8r 1) wi ka AS 
xkkm(6,1)3*sim( 5X4 KK Oi a) Km ee oli 

xkkm (7,1) *4*%* sam (4 (xk kim Ge E 


$transpose of the Jacobian 

Ht (:,1+1)=[sin(xkkm(8,1)*1+xkkm(9,1)); 
sin(2*(xkkm(8,i)*i+xkkm(9,i))); 

Sin (3* (xkKkm(6,1) “2 +xk emo eee 

sin (4* (<kkm (8,1) ir kk ee 

COs (2* (xkkm(8, 1i) ItTtxkkMm o 

Cos (3*(xkkm(87 1) ini e ee 

cos (4*(xkkm(8, 1) *1:xkkn 

e 

TO cre 


% Jacobian 
A 


$Measurement update of error covariance 
Pkk= (eve (9) —“K( aye kam 


v(:,1) = ran (I te 7 
rand(1)*1e-2 
rand(i)y^tec2 
rano e 
rene) te 2 
randı) ve 2 
rand (1) *1e-2 
rana (1) *1e-4 

pana ee: 


$State update 

x(:,2T*1)58355 065 pop D 

end 

plot (t,4(1.131),t7xkk(l Ian label ean ae 

ylabel (‘Amplitude’),gtext(‘-True State’),gtext(‘--Estimate 
of the State’),meta tez52cl 
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ME EK 15:3). t, Xkk(2,1:1));pause,grid,xlabel( 'Time'); 
pmuabel(vAmplrtude').,gtext('-True State'),gtext( --Estimate 
Crime state”) meta tez52c2 


>, lay, t,xXkk(5,1:1)) a pause grid, xL1abel( Time’); 
ylabel('Amplitude'),gtext('-True State'),gtext('--Estimate 
of the State'),meta tez52c3 


pot, (4, 1:01), 6, XKKk(4,1:1));pause, grid, xlabel ( Time”), 
ylabel('Amplitude'),gtext('-True State'),gtext('--Estimate 
Geeene State“), meta tez52c4 


EE lone pty Kho Sl pausesgridxlabel Timer 
cbpejcÓ'Amplitude'),gtext('-True State'),gtext(' --Estimate 
of the State’'),meta tez52cs5 


PE oO, dol) C, xkk(6,1+1)) pause, grid, xlabel (“Time’ j> 
ylabel('Amplitude'),gtext('-True State'),gtext('--Estimate 
of the State'),meta tez52c6 


pat, (7. l. 1, tz 2kkC7, 171) pause; grid, xlabel (Time), 
y label('*Amplitude'),gtext('-True State” ) ,gtext ('--=Estimate 
of the State”) meta tez52c7 


PRO (1. 2(8,1:1)=xkk (8,1:1)) ;pause,grid,xlabel(*Time“), 
ubel(VPhase Eror'),gtext('--Estimate of the State'),meta 
tez52c8 


Pi, O LS t Kk(9 11) )/paüso;dge ld; 
xlabel('Time'),ylabel('Phase Eror'),meta tezla9 
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APPENDIX D 


9o This program trackes the frequency, amplitude, and the phase of the nonperiodic 


signal in noise which consists of two harmonics. It uses extended Kalman filter algorithm 


for the rectangular model. 


% 


% State space model 


ESO 0 0099050209080 
O IOO 0 0 0 0m0 
Sp ADA 
mp co OON O0 
005000000 
(ooo OOO 
e ge ORN 
re 0) 
(00995 919 0 0 li) 

Initial conditions of the true state 


a(1)=2.7;%Amplitude of the first component. 
a2(1)=0;%Amplitude of the second component. 
a3(1)=a(2)/2;%Amplitude of the third component. 
a4(1)=0;%Amplitude of the fourth component. 
w(1)=2*p1*0.04;SFundamental frequency. 
01(1)20.01;$Phase of the first component 
b2(1)-a(1) /2;5Phase of the second component 
b3(1)=0;%Phase of the third component 
b4(1)=0;%Phase of the fourth component 


sInitial condition of the estimation 


xkkm(:.1>224-02020:124500 025000 
R=0.1; % Measurement noise 


gi-le-3; % process noise 
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q3=le-7; 

dal © 
W cb 
Jag | 
Q0 0 
0000 
ON DTO 
000 
OYU 
000 


© OOTO 


OOO 10 


G © (© 2 C9 
O OOO © 
OD OTC 5063. 69 C23 


il 


CAD Ge a 


q 
0 g2 
00 liz 


O 


Lor covariance matrix. 


Pkk-[1 0 


¡Do a OSORO 


¡5 OS a OTO 


G 


0 


O O OT OIG O OMD 
C s Oo O (CI 


OI DIAS CE 


UNA 


O O 
©: C 
© © 
Coe 
Co 


Z 000400 


OT OOT €» 
OO MOS O 


T ae obian matrix. 
Hus o-—-[0. 0 00 0.0 0 0 O0]; 


$Transpose of the Jacobian matrix. 
mue 1)—r0-0 U © YOU O0 0 0]*; 


t=1:450;%Number of samples 


STrue state matrix 


x(:,1)=[ 


a (1) 

az (iy 
a3(1) 
a4(1) 
BZ) 
Do) 
b4 (1) 
w(1) 

O1(1) 


If 


71 


Bor i~ 1:450 
rand(‘normal’ ) 


zZ(i)=x (1,1) *sin(x (8,1) *i+x(9,1))+x(2,1) *Sin(2* (x(8 7 a 
1)))+x (3,1) *sin(3* (x (8,1) *1+x (9,1) ))+x (4,1) *sin(4" -= 
x (9,1)))+x(5,1) *cos (2% (x (8,1) *i +x (9, 1) ) ) +x (6,1) *Cocite ae eee 
)*i+x(9,1)))+x(7,1) *cos (4* (x (8,1) *i1+x (9,1) ))+0.1* ranciane 


$Variable 
je(i)-xkkm(1,i)*sin(xkkm(8,i1)*i-c-xkkm(9,1)) *xkkm(2,1) 9D NP LI 
xkkm(8,1)*1+xkkm(9,1)))+xkknm(3,1)*sin(3* (xkkm(8, yen 
1)))+xkkm(4,1)*sin (2 ("AS eon E 


$SVariable 
ke (1) =xkkm(5,1) *cos (2* (xkkm(8, 1) *1+xkkm(9,1) ) ) *xkkm (6D ENS E 
(3* (xkkm(8,i)*i+xkkm(9,1)))+xkkm(7,i)*cos (4* (xkkm(8,1) *i*xkk 
(e a7 


he(i)=je(i)+ke(i); 


$Time update of the error covariance 
PkkmsE *Pkk*P”' +0; 


$ Kalman gain 
K(:,1)sPXkkm*H(i,:)'^*ipns ea USUS 


$ Estimate update 
xkk({:,i)=xkkm(:, I) K zn e e 
xkkm(s, a+ 1)Sk* xkk (oe 


$Variable 
C(i)sxkkm(l,i)*i*cos(xkkm(8,i)*i-c*xkkm(9,1)) *xkkm(2,1) 2x2 = 
s (2* (xkkm(8,i) *itxkkm(9,i)))+xkkm(3,1) *3*1%*cos (3* (xkkm (G7 
it+xkkm(9,i)))+xkkm(4,i) *4*1i*cos (4% (i*xkkm(8, i) +xkkm(9, LI 


SVariable 
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mu -sxkkm(o,1)*2*)*5130n4Z2*(xkkm(8,1)*^1f4xkkm(9,1)))- 
plo (6, 4) * 3:1 sin(3*(xkkm(8,1)*i+xkkm(9,1)))- 
ms 1) *4*1*sin(4*(xkkm(6,1i)*itrxkkm(9,1))): 


$Variable 

Pies kin( ll) 1) *"cos(xkkm(8, 1) *i+xkkm(9,1))+xkkm(2,1)*2*cas1(2* 
 em(o 1) i+ xkkmn( 9,1) ))+xkkm(3,2)*3*eos (3* (xkkm(e 1) *itekkm 
vw a) ee elem (4,1) *4* cos (4* (re xkkm( oor exh khm (9) 2)))- 
SENI OD, 1)*2*5s1n(2*10xXkkm(9.1) X -xkkm(9,1)))— 

EDEN 1)53*s1n(3"*(xkkm(8,1)"3t*xkkmit9,1)))- 

EU )*24*sxn(4*(xkkm(85,a)"*1itxkkmi9,1))); 


$transpose of the Jacobian 

e Iey— (SI (XKKM US, 1.) “rt xkkm(9S,2)') > 
when (xe km 6, 1) i tkxkkm(9,; ae 

Em Ooosem (d, 1) *ixxkkmi9,1))) 5 
sin(4* (xkkm(8, 15 Mob 
cos (2* (xkkm(8,1)*1+xkkm(9,1))); 
coso" (xkkm(s,1)*1+xkkm(9,1))); 
co 4: (xkkm(6,1i)*i+xkkm(9,1))); 
BA AL). 

memo (1) -i 


, 


/ 


5Jacoblan 
H(i+1,:)=Ht(:,i+1)”’ 


$Measurement update of error covariance 
ENM-(teve(9) — K(t,;1) *H(1,:)) ^Pkkm; 


Ha uu) = Leand(l)*le-2 
rand (1) *~1le=Z 
rand (1) *le-Z 
rand) bez 

randtil)*19-2 

pand(l)*1e-2 
ana lem 
rana t1) *le=4 
rand (l1) *le=4]; 


%State update 
SA Eo fed T5366: 1) se 
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end 

plotí(t,x(1,1:1),t, xk ge a UCM 

ylabel (‘Amplitude’),gtext(‘-True State’),gtext (‘--Estimate 
ot the State”), meta tezsZal 


plotí(t,x(2,1:i1),t,xkk(2,1:1));pause,grid,xlabel 0m AT 
ylabel('Amplitude'),gtext('-True State'),gtext("'--Estimate 
of the State’),meta tez52c2 


plot (t,x(3,1:i),t,xkk(3,1:1));pause, grid, xlabel( em 
ylabel ('Amplitude'), gtext (*'-True State’),gtext (‘--Estimate 
of the State“ ),meta tez52c3 


plot (t,x(4,1:1),t,xkk (4,121) );pause, grid, xlabel (“Time ae 
ylabel (‘Amplitude’), gtext(‘-True State’), gtext (‘--Estimate 
of the State’),meta tez52c4 


plot (t,x(5,1:1),t,xkk(5,1:1)); pause, grid, xlabel ( “lime ae 
ylabel (‘Amplitude’), gtext(‘-True State’),gtext (‘--Estimate 
of the State’),meta tez52c5 


plot (t,x(6,1:1),t,xkk(6,1:1));pause, grid, xlabel( Tinea 
ylabel (‘Amplitude’) ,gtext(‘-True State’),gtext (‘--Estimate 
of the State’),meta tez52c6 


plot (t,x(7,1:1),t, xkk(7,1:1));pause, grid, xlabel ( “Tinea 
ylabel ( ‘Amplitude’ ),gtext(‘-True State’),gtext (‘--Estimate 
of the State'),meta tez52c7 


plot (t,x (8,1:1)-xkk(8, 171) ); pause, grid, xlabel( Tum M 
ylabel (‘Frequency’), gtext(‘--Estimate of the State’),meta 
tez52c8 


plot(t,x(9,1:i1),to xk pls mn 
xlabel(‘Time’),ylabel(‘Phase Eror’),meta tez52c9 
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